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Abstract.  Most  robot  control  and  planning  algorithms  are  complex,  involving  a  combination 
of  reactive  controllers,  behavior-based  controllers,  and  deliberative  controllers.  The  switching 
between  different  behaviors  or  controllers  makes  such  systems  hybrid,  i.e.  combining  discrete 
and  continuous  dynamics.  While  proofs  of  convergence,  robustness  and  stability  are  often 
available  for  simple  controllers  under  a  carefully  crafted  set  of  operating  conditions,  there 
is  no  systematic  approach  to  experimenting  with,  testing,  and  validating  the  performance  of 
complex  hybrid  control  systems.  In  this  paper  we  address  the  problem  of  generating  sets  of 
conditions  (inputs,  disturbances,  and  parameters)  that  might  be  used  to  ’’test”  a  given  hybrid 
system.  We  use  the  method  of  Rapidly  exploring  Random  Trees  (RRTs)  to  obtain  test  inputs. 
We  extend  the  traditional  RRT,  which  only  searches  over  continuous  inputs,  to  a  new  algo¬ 
rithm,  called  the  Rapidly  exploring  Random  Forest  of  Trees  (RRFT),  which  can  also  search 
over  time  invariant  parameters  by  growing  a  set  of  trees  for  each  parameter  value  choice.  We 
introduce  new  measures  for  coverage  and  tree  growth  that  allows  us  to  dynamically  allocate 
our  resources  among  the  set  of  trees  and  to  plant  new  trees  when  the  growth  rate  of  existing 
ones  slows  to  an  unacceptable  level.  We  demonstrate  the  application  of  RRFT  to  testing  and 
validation  of  aerial  robotic  control  systems. 


1  Introduction 

Hybrid  systems  provide  mathematical  models  of  discrete/continuous  dynamic  sys¬ 
tems.  Many  robotic  systems  including  walking  robots,  grasping  and  manipulation, 
or  logic-based  software  controlled  robots  can  be  modelled  under  this  framework.  In 
fact  most  robot  control  and  planning  algorithms  are  complex,  involving  a  combi¬ 
nation  of  reactive  controllers  [2],  behavior-based  controllers  [23],  and  deliberative 
controllers  [11,16].  While  it  is  possible  to  analyze  each  controller  in  isolation  it  is 
well  known  that  the  interaction  between  discrete  and  continuous  time  dynamics  of 
such  systems  can  produce  rich  and  often  unexpected  behavior.  For  this  reason,  as 
these  systems  grow  in  complexity  and  sophistication,  the  need  for  automated  design 
tools  increases.  The  focus  to  date  in  the  literature  has  been  on  the  formal  verification 
of  safe  operation,  via  the  solution  of  the  reachability  problem,  initially  through  sym¬ 
bolic  methods  [27,17]  and  later  through  numerical  techniques  [3,1,5,24,9].  How¬ 
ever,  it  soon  became  apparent  that  the  class  of  hybrid  control  systems  for  which 
the  reachability  problem  was  decidable  is  quite  limited  in  both  expressiveness  and 
dimensionality. 
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Test  generation  -  a  well  established  concept  for  software  design  -  is  a  relatively 
new  approach  to  analyzing  control  systems.  Rather  than  prove  controller  safety  ex¬ 
haustively,  our  approach  is  to  try  to  generate  a  set  of  test  scenarios,  using  Rapidly 
Exploring  Random  Trees  (RRT),  that  cause  the  system  to  fail.  In  addition  to  con¬ 
sidering  traditional  continuous  inputs,  we  have  extended  the  method  to  consider 
uncertain  time  invariant  parameters.  We  call  our  algorithm  the  rapidly  exploring 
Forest  of  Trees  (RRFT).  The  merit  of  this  approach  as  compared  with  reachability 
analysis  is  that  decidability  issues  do  not  come  into  play  because  we  are  not  at¬ 
tempting  to  represent  or  manipulate  a  reachable  set.  The  drawback  of  the  approach 
is  that  it  is  a  semi -decision  method,  meaning  that  we  can  only  disprove  system  safety 
by  counter-example  -  safety  cannot  be  proved.  Despite  this  drawback  we  feel  that 
randomized  approaches  hold  the  most  promise  for  addressing  complex  nonlinear 
real-world  problems  for  which  trial  and  error  testing  is  not  sufficient;  and  formal 
analysis  is  intractable.  Similar  work  has  recently  appeared  which  uses  genetic  al¬ 
gorithms  [26],  Some  works  have  used  RRTs  as  a  synthesis  tool  for  hybrid  control 
systems  [10,4];  but  the  idea  of  using  RRTs  to  explore  a  system’s  faults  has  only 
appeared  in  passing  [15,8]. 

Our  approach  consists  of  drawing  a  parallel  to,  and  using  methods  from,  motion 
planning.  Informally  the  motion  planning  problem  is:  given  a  robot  with  dynamics 
and  constraints  (obstacles),  to  find  a  path  (if  one  exists)  from  the  starting  configura¬ 
tion  to  the  goal  configuration  of  robot  in  some  complex  high  dimensional  configu¬ 
ration  space.  Similarly,  the  goal  of  test  generation  is  to  find  a  sequence  of  inputs  (or 
disturbances)  and  parameters  (if  one  exists)  which  will  take  a  hybrid  system  from  an 
initial  state  to  some  unsafe  set  in  the  hybrid  state  space.  Interestingly  motion  plan¬ 
ning  research  experienced  a  similar  evolution  from  exact  (symbolic)  methods  [28], 
followed  by  the  result  that  the  problem  was  fundamentally  hard  [6],  to  a  shift  toward 
approximate  methods  that  worked  well  in  practice  [25].  Most  recently  research  ac¬ 
tivity  focuses  on  randomized  approaches  to  the  problem  which  have  been  shown  to 
scale  well  with  dimension. 

The  primary  differences  between  motion  planning  and  testing  he  in  the  types  of 
systems  considered.  For  example,  motion  planning  approaches  do  not  traditionally 
consider  hybrid  systems  (though  recent  work  has  [10]).  Another  difference  is  that 
in  motion  planning  problems  the  state  space  is  not  simple  connected,  in  the  geo¬ 
metric  sense,  due  to  the  presence  of  obstacles,  necessitating  the  use  of  sophisticated 
collision  detection  algorithms.  For  hybrid  system  the  state  space  is  usually  simply 
connected  with  a  given  mode.  Perhaps  most  importantly,  robotic  systems  are  almost 
always  output  controllable  (by  design),  so  the  reachable  space  is  the  entire  output 
space.  Therefore  a  solution  usually  exists,  unlike  testing  problems.  As  a  result  con¬ 
siderations  of  when  to  stop  growing  the  tree  are  rarely  discussed. 

The  contributions  and  outline  of  this  paper  are  as  follows 

•  Formally  introduce  the  Test  Generation  problem  for  complex  control  systems, 

point  out  the  similarities  to  motion  planning  (Sections  2-3). 

•  Define  new  coverage  criteria  for  RRTs  (Section  4). 
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•  Introduce  the  RRFT  algorithm  which  is  capable  of  searching  both  over  tradi¬ 
tional  continuous  inputs  (like  the  RRT)  and  uncertain  time  invariant  parame¬ 
ters.  RRFT  modifies  its  search  strategy  based  on  the  run  time  coverage  estimates 
(Section  5). 

Incidentally,  the  algorithm  can  be  used  for  motion  planning  or  testing.  Finally  we 
apply  the  algorithms  to  validate  two  controllers  for  robotic  unmanned  aerial  vehicles 
in  6. 


2  Problem  Statement 

Definition  1.  We  define  a  Finite  Time  Hybrid  Control  System  with  unknown  pa¬ 
rameters  dependencies  as  (modified  from  the  Hybrid  Automata,  see  [22])  as  a  tuple 

H  =  (X,  Q,  U,  T,  P,  Init,  /,  Inv,  E,  G,  R )  where 

•  X  C  is  a  set  of  continuous  variables; 

•  Q  C  N  is  a  set  of  discrete  variables  which  index  the  system  modes; 

•  U  C  Rm  is  a  compact  set  of  continuous  input  values; 

•  T  =  [to,  tf]  C  R  is  a  compact  time  interval  the  system  evolves  over; 

•  P  C  Rp  is  a  compact  set  of  uncertain,  time  invariant  parameters; 

•  Init  C  Q  x  X  is  a  set  of  possible  initial  conditions; 

•  f:QxXxUxP—>  Rw  is  a  vector  field  which  prescribes  the  time  derivative 
of  the  continuous  variables  ( i.e .,  x  =  f(q,  x,  u\p))\ 

•  Inv  :  Q  — >  2X  assigns  to  each  q  £  Q  an  invariant  set; 

•  P  C  Q  x  Q  is  a  collection  of  edges  describing  the  possible  discrete  transition 
(a.k.a.-  mode  switches); 

•  G  :  E  — >  2XxP  assigns  to  each  e  =  ( q ,  q')  £  E  a  guard;  and 

•  R  :  E  x  X  — >  2a  assigns  to  each  e  =  (q,  q')  £  E  a  reset  relation. 

Throughout  this  paper  we  refer  to  (x,  q)  as  the  state  of  the  hybrid  system.  Note  that 
we  use  the  term  “input  signal”  in  the  most  general  sense  in  that  it  can  include  yet  un¬ 
specified  feedback  control  inputs,  human  in  the  loop  type  inputs,  disturbances,  etc. 
Note  that  the  uncertain  parameters  can  affect  the  continuous  or  discrete  dynamics. 
Again,  many  robotic  system  can  be  modelled  in  this  way  (see  Sect.  1).  Examples  of 
P  could  include  a  control  gain,  the  initial  condition  of  an  adversarial  agent,  or  the 
width  of  a  narrow  passage. 

Definition  2.  The  Testing  Problem  TP  is  specified  as  a  tuple  ( H ,  x°,q°,  s,  U,  St) 
where 

•  H  is  a  finite  time  hybrid  control  system  as  described  above; 

•  x°,  q°  €  Init ; 

•  U  is  user  defined  discretization  of  [/; 

•  St  is  the  fixed  time  period  for  which  a  constant  u  £  U  is  applied  such  that 
(t  f  —  t0) /St  =  k  is  an  integer; 

•  xQxP->Risa  specification; 
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Given  an  initial  state  and  a  particular  control  function  u(t),  s  can  be  used  to  define 
a  set.  If  s(x,  q;p)  >  0,  then  (x,  q)  is  an  acceptable  state  inside  the  specification  set 
for  p;  otherwise  it  is  unacceptable. 

Problem  1.  Given  an  initial  state,  (a;0,  <7°),  the  Testing  Problem  is  to  determine 
{ui,U2,  ■  ■  -  Uk}  €  U,  which  define  a  piecewise  constant  control  sequence 

u(t)  =  ut  if  (i  —  1)  •  8t  <  t  <  (i)  ■  St  (1) 

for  i  =  1 , , . . ,  k,  and  p  £  P,  if  they  exist,  such  that  3t  £  T  for  which  s(x(t),  q(t)\p)  < 

0. 

In  words,  the  goal  of  the  test  generator  is  to  determine  a  counter-example  -  an  piece- 
wise  constant  input  sequence  and  a  value  of  the  time  invariant  uncertain  parameters 
which  will  cause  the  system  to  fail  -  if  one  exists.  Note  the  similarity  to  trajectory 
planning  (for  example  see  problem  statement  in  [7]) 

3  Testing  Through  Rapidly  Exploring  Random  Trees 

The  similarities  between  the  Testing  Problem  and  the  motion  planning  problem, 
suggest  the  use  of  a  randomized  methods  such  as  Probabilistic  Road  Maps  [13] 
or  Rapidly  Exploring  Random  Trees  (RRTs)  [19].  We  choose  the  RRT  primarily 
because  it  works  directly  with  the  set  of  admissible  inputs  and  is  therefore  di¬ 
rectly  applicable  to  systems  with  complex  dynamics.  This  algorithm  has  experi¬ 
enced  widespread  success  in  solving  a  variety  of  high  dimensional  and  nonlinear 
problems  in  motion  planning  and  has  recently  been  applied  to  controller  synthesis 
problems  for  hybrid  systems  [10,15,4].  Figure  1  illustrates  the  concepts  and  a  very 
basic  algorithm  is  given  in  Algorithms  1  and  2.  Note  that  p  is  a  suitable  metric  func¬ 
tion;  and  the  notation  (, x ,  q)  +  fSt  H(u)dt  means:  using  x,  q  as  an  initial  condition, 
simulate  the  evolution  of  the  hybrid  system  for  St  seconds  using  u{t)  as  the  control 
input.  Various  versions  of  the  algorithm  can  be  generated  using  different  metrics,  or 
random  distributions.  In  Sect.  5  we  focus  on  stopping  criteria  if  no  solution  is  found. 


Algorithm  1  Grow  Test  Set  T 
Initialize  RRT:  T.addvertex(a:0,  q°) 
while  fi(x,  q)  £  T  such  that  s(x,  q)  <  0  do 
Extend(T) 

end  while 


4  Coverage  Measures 

It  has  been  shown  that,  for  a  controllable  system,  the  RRT  will  ultimately  cover  the 
entire  state  space  as  the  number  of  sample  points  goes  to  infinity  [20] .  Unfortunately, 
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Fig.  1.  The  Testing  algorithm  (inspired  by  the  RRT  [19]).  The  test  set  is  represented  as  a  tree 
T  with  nodes  as  states  (x,  q )  and  edges  as  inputs  u  €  U.  First  a  new  state  is  generated  at 
random,  xrand,  qrand-  The  algorithm  then  determines  the  closest  state,  xnear,  qnear  in  the 
tree  to  the  random  state  (left).  It  determines  which  u  £  U  brings  x near,  qnear  closest  to 
Xmnd,  qrand  (center).  Unew  is  applied  for  a  duration  St  and  the  new  node  xnew,  qnew  and 
edge  Unew  are  added  to  the  tree  (right). 


Algorithm  2  Extend(T) 

Xrand,  qrand  <—  random() 

Xnear ? Qnear  <—  nearestNeighbor(T,  {xrand,  qrand)  ) 

Unew  =  cirg  (x  r  and  i  Qrand')  5  near  5  Qnear')  +  JdtH(u)dt)} 

(jXnew  1  Qnew  )  — '  {Xnear  5  Qnear')  "b  /  H(u  new  (■ t))dt 

T.add  vertex(Xnew ,  qnew) 

T.addEdge(Mnew  ,  (x  near,  qnear)  *  {.Xnew ,  qnew)) 


because  many  of  the  systems  we  consider  are  not  controllable  with  respect  to  the 
test  inputs,  the  reachable  set  is  not  the  entire  space.  It  is  very  difficult  in  practice 
to  estimate  coverage  quality  because  the  reachable  set  is  not  known  a  priori.  It  is 
therefore  important  for  us  to  estimate  coverage  for  two  reasons. 

•  Many  testing  problems  may  have  no  solution,  meaning  that  there  is  not  a  counter¬ 
example  to  be  found.  In  such  a  case  we  must  decide  when  to  terminate  Algo¬ 
rithm  1. 

•  It  is  possible  for  Algorithm  1  to  get  stuck  in  “local  minima”  due  to  its  greedy 
strategy  [8]  or  to  slow  down  because  a  tree  is  fully  grown. 

Regarding  the  second  point,  we  can  use  coverage  measures  to  determine  when  it 
might  be  appropriate  to  alter  the  search  strategy.  Indeed  we  explore  this  further  in 
Section  5 

4.1  Coverage:  previous  work 

It  has  been  pointed  out  many  times  [18]  that  coverage  of  X  by  T  is  related  to  the 
Voronoi  Diagram  of  the  vertices  of  the  tree.  While  this  connection  is  useful  for 
theoretical  analysis  the  major  problem  is  that  it  is  impractical  to  compute  Voronoi 
diagrams  in  dimensions  over  2.  The  Discrepancy  (a  concept  from  the  Monte  Carlo 
literature)  is  also  mentioned  in  [18]  but  it  too  is  very  difficult  to  compute.  Another 
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appealing  idea  to  measure  the  growth  or  coverage  of  the  RRT  is  to  compute  the 
volume  of  the  convex  hull.  Unfortunately  the  convex  hull  is  more  indicative  of  the 
distribution  of  the  points  than  it  is  of  the  coverage.  For  example,  in  Figure  2  the  left 
and  right  panels  represent  two  sample  sets  whose  convex  hulls  are  identical.  Obvi¬ 
ously  the  sample  shown  to  the  right  covers  the  state  space  better.  In  [21]  a  variant 
of  the  convex  hull  is  explored.  Rather  than  compute  the  hull  of  all  tree  vertices,  ver¬ 
tices  are  grouped  according  to  their  depth  from  parent  nodes.  The  union  of  these 
hulls  clearly  provides  a  better  approximation  however  the  selection  of  the  group¬ 
ing  is  somewhat  arbitrary.  It  is  not  clear  how  to  relate  the  union  to  coverage  due  to 
possible  overlaps. 


Fig.  2.  Two  sample  sets  which  have  the  same  convex  hull.  The  set  on  the  left  clearly  has 
inferior  coverage  to  the  set  on  right. 


Fig.  3.  Two  sample  sets  which  have  the  same  dispersion  (the  size  of  the  largest  empty  ball, 
drawn  with  dashed  line).  The  set  on  the  left  clearly  has  inferior  coverage  to  the  set  on  right. 


It  appears  that  the  most  accepted  measure  to  date  is  the  Dispersion  (see  [12]  or 
more  recently  [18]).  Assuming  we  have  a  sample  set  X,  which  contains  N  points, 
over  the  space  X,  it  is  defined  as 

p(X,  p)  =  sup  min  p(x,  x)  (2) 

xex  sex 

and  can  be  thought  of  as  the  radius  of  the  largest  empty  ball  in  X  and  obviously 
depends  greatly  on  the  choice  of  metric  p.  While  its  use  has  been  advocated  for  an¬ 
alyzing  planners  we  reject  it  for  computation  on  two  grounds:  (1)  it  is  impractical  to 
compute  in  high  dimensions;  and,  (2)  it  is  an  overly  conservative  coverage  measure 
because  it  only  considers  the  largest  ball.  For  example,  in  Figure  3  the  left  and  right 
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Fig.  4.  A  grid  is  super  imposed  on  the  state  space.  The  shaded  regions  indicate  unreachable 
sets.  The  distances  from  the  grid  points  to  the  closest  nodes  are  djCshown  as  dashed  arrows) 
and  the  grid  spacing  is  5. 


panels  represent  two  sample  sets  with  the  same  dispersion.  Obviously  the  sample 
shown  on  the  right  covers  the  state  better. 

4.2  New  Coverage  Measures 

We  have  three  goals:  to  measure  the  coverage  of  the  state  space  X  by  the  tree  T ; 
to  measure  the  growth  of  the  tree  T  and  to  measure  the  coverage  of  the  set  of  time 
invariant  parameters  P  by  a  countable  finite  set  of  values  P. 

RRT  Coverage  We  begin  by  overlaying  a  grid  of  ng  points  and  spacing  <5  on  the 
state  space.  We  calculate  the  minimum  distance  from  each  grid  point  j  to  the  set 
of  nodes  in  the  tree,  dj.  The  quantity  min(d,  ,  S)  may  be  thought  of  as  the  radius 
of  the  largest  ball  centered  at  each  grid  point  which  does  not  contain  a  tree  node  or 
adjacent  grid  points  (see  Figure  4).  Given  a  tree  T  we  define  its  coverage,  c(T),  as 

=  1  i  mmid^ 

SU  "» 

which  is  the  average  of  all  the  node  distances,  normalized  by  the  grid  spacing. 

Our  measure  is  similar  to  an  approximation  of  an  “average”  dispersion,  but  far 
less  conservative  and  faster  to  compute.  Clearly  this  measure  is  a  monotonically 
decreasing  function.  If  it  goes  to  zero  on  a  given  grid  it  tells  us  that  any  set  whose 
distance  along  its  smallest  dimension  is  greater  than  the  grid  spacing  has  been  en¬ 
tered.  Said  another  way,  the  state  space  is  covered  up  to  a  resolution  equal  to  the  grid 
spacing.  Overall  one  of  the  advantages  of  this  measure  is  that,  the  grid  size  can  be 
as  fine  or  coarse  as  one  chooses.  Finer  grids  will  require  more  distance  queries  but 
are  more  accurate  indications  of  coverage.  Of  course  grids  can  be  generated  in  the 
“output”  or  specification  space  to  measure  coverage  there  as  well.  From  a  compu¬ 
tational  point  of  view  it  should  be  stressed  that  this  list  of  distances  can  be  updated 
incrementally  as  new  tree  nodes  are  added,  since  the  affect  of  each  new  node  is 
local. 
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RRT  Growth  The  derivative  of  c(T)  with  respect  to  the  number  of  vertices  in  the 
tree,  nv,  indicates  the  growth.  Therefore 

g(T)  =  -dc(T)/dnv  (4) 

In  practice  the  derivative  is  actually  a  finite  difference  and  we  may  choose  to  look 
at  the  change  in  c  over  the  course  of  adding  several  new  vertices  to  T. 

Time  invariant  parameter  set  coverage  The  set  of  parameters  over  which  we  may 
tests  the  system  may  be  generated  directly  since  they  are  not  subject  to  differential 
constraints  and  therefore  the  sample  set  can  be  engineered  to  have  a  certain  coverage 
of  P.  Halton  sequences  [12]  have  naturally  low  dispersions  and  are  cheap  to  com¬ 
pute.  Therefore  we  define  the  coverage  of  P  using  the  dispersion  defined  in  eq.(2) 
normalized  by  pmax . 

5  Forest  of  Random  Trees  Algorithm 

The  original  RRT  given  in  Alg.  1  only  addresses  time  varying  inputs  such  as  u(t). 
Recall  that  the  evolution  of  our  system  is  characterized  by  time  invariant  parameters, 
p  £  P  as  well.  In  our  RRFT  algorithm  (see  Alg.  3),  the  repeated  application  of  the 
RRT  algorithm  results  in  a  tree  for  every  choice  p  £  P  (called  the  seed  value). 
Accordingly,  we  need  to  consider  a  set  of  trees  (a  forest)  that  rapidly  explore  the 
state  space.  We  call  a  RRT  grown  (or  rooted)  at  pi  £  P,  TPi.  Initially  we  plant 
trees  at  P  =  {pi, . . .  ,  p„t},  where  n*  is  the  maximum  number  of  active  trees  we 
can  consider  concurrently.  At  any  point  if  a  counter  example  is  found  (a  state  and 
parameter  s(x,  q ;  pf)  <  0  for  which  (x,  q)  £  TPi)  the  algorithm  terminates. 

Because  we  have  limited  computational  resources,  we  must  decide  how  to  allo¬ 
cate  them  in  growing  the  trees  -  choosing  which  to  grow  and  which  to  terminate. 
As  the  RRT  algorithm  progresses,  we  monitor  the  progress  of  each  tree.  If  at  any 
point  the  growth  of  one  of  the  trees  as  measured  by  g{Tpi )  drops  below  a  threshold 
g,  the  tree  is  considered  no  longer  actively  growing;  or,  if  the  coverage  c(Tpi)  is 
less  than  a  threshold  c,  the  tree  is  considered  fully  grown.  In  either  case  the  tree  is 
terminated.  Provided  the  set  P  is  not  adequately  covered  with  seeds  (as  measured 
by  the  dispersion)  a  new  “seed”  is  planted  and  a  new  tree  is  initiated.  The  process 
of  planting  and  growing  new  trees  continues  until  a  counter  example  is  discovered, 
or  until  P  is  sufficiently  covered  (p(P)  <  ft)  with  seed  values,  whose  trees  have 
stopped  growing. 

One  key  component  of  this  approach  is  that  each  RRT  can  be  computed  in  par¬ 
allel  on  a  different  CPU’s,  therefore  we  assume  a  fixed  computational  resource  that 
will  dictate  the  number  of  trees  that  can  be  simultaneously  computed  in  parallel. 

6  Examples 

We  demonstrate  the  algorithm  on  two  examples  involving  Robotic  Unmanned  Arial 
Vehicles. 
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Algorithm  3  Test  Generation:  Tp  =  RRFT(fT,  x°,  q°,  s,  U ,  g,  c,  (i.  nt) 

Generate  initial  seed  set  P  =  {p\ , ,  pnt }  where  pi  £  P 
for  i  =  1, . . .  nt  do 

Initialize  RRT:  T^.addvertexfr0,  q°) 

end  for 

while  nt  ^  0  do 

for  i  =  1, ...  nt  do 
Extend(7^ ) 

if  3(x,  q )  £  TVi  such  that  s(a:,  p)  <  0  then 
return  TPi 

break  (test  case  found) 
else 

if  g (TPi )  <  g,  OR,  c(TPi)  <  c  then 
terminate  TPi 
nt  <— nt  —  1 
if  /./  ( P  )  >  ft  then 
nt  <—  nt  +  1 

Generate  new  pt  £  P  via  Halton  sequence  and  append  to  P 
Initialize  RRT:  TPi  .addvertex(x°,  q°) 

end  if 
end  if 
end  if 
end  for 
end  while 


6.1  Example  1:  Aircraft  conflict  resolution 

As  a  first  example  we  test  an  aircraft  collision  avoidance  protocol  proposed  in  [24], 
We  test  over  a  continuous  input  (u(t),  a  wind  disturbance)  and  a  constant  parameter 
(p,  the  minimum  separation  distance)  and  consider  a  scenario  involving  5  aircraft. 
The  problem  has  15  states,  which  is  considerably  larger  than  problems  which  have 
been  considered  in  the  literature  on  reachability  and  verification. 

Each  aircraft  has  three  states,  Xt  =  (x,y,ff)  and  there  are  5  aircraft  so  the 
continuous  state  space  is  X  =  X\  x  . . .  X$.  The  continuous  dynamics  /  :  Q  x  X  x 
U  x  P  are 

±i  =  v  cos (9i)  +  (-di  sin(#j)  +  d2  cos (0i))(-  sin(6lj))  (5) 

y%  =  v  sin (0*)  +  (— di  sin(6>j)  +  d2  cos(6»i))(cos(0i))  (6) 

9i=u(q;p)  (7) 

Where  v  is  a  constant  forward  velocity;  u  =  [di  d2]T  £  [— w,  w]  x  [— w,w]  is  a 
wind  disturbance  whose  normal  component  to  the  planes  alters  their  dynamics  (this 
is  the  main  difference  versus  [24]).  Note  that  q  and  p  do  not  explicitly  appear  in 
the  dynamics  but  rather  determine  u>,  the  preset  yaw  rate  control  law.  The  control 
law  was  designed  to  bring  each  plane  from  I nit  to  its  own  predetermined  final 
destination  (xf,yf)  without  colliding.  The  function  u>  switches  depending  on  the 
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mode.  At  the  start  positions,  the  aircrafts  are  in  q  =  1,  ( heading  mode)  and  rotate 
until  pointing  toward  their  goal  positions,  so  ut(l)  =  0goai  —  0,.  Once  they  reach  the 
desired  heading,  they  switch  to  q  =  2  ( cruise  mode),  u>(2)  =  0,  and  cruise  straight 
toward  the  goals.  If  two  aircrafts  get  within  a  distance  p  km  of  each  other,  each  of  the 
two  aircraft  enters  q  =  3  ( avoid  mode)  and  makes  instantaneous  —90°  turns,  then 
it  follows  a  half  circle  with  angular  velocity  w(3)  =  c.  After  finishing  the  circular 
turn,  they  make  instantaneous  turns  again  until  pointing  to  their  own  goal  positions 
and  return  to  cruise  mode.  In  case  the  aircraft  sees  another  aircraft  within  p  km 
during  the  avoid  mode,  it  makes  —90°  turn  again  and  executes  the  same  operation 
as  above.  This  is  illustrated  in  (see  Figure  5  left).  The  specification  is  the  minimum 
distance  between  all  pairs  of  planes. 

When  ||u||  <  0.03fcm/sec  and  p  =  5.25 km  a  collision  among  the  aircraft 
was  discovered  (see  Figure  5  right)  after  about  8,600  nodes  and  5  parameters  were 
explored.  A  uniform  distribution  was  used  to  generate  samples,  and  a  simple  metric 
based  on  a  weighted  Euclidean  distance  is  utilized 

P  =  d  +  wa\A9\  (8) 

where  d  is  a  Euclidean  distance  between  two  (x,y)  positions,  A0  G  ( — 7r,  7r]  is 
a  heading  difference,  and  wa  is  a  weight  factor.  Figure  6  shows  c(T)  and  g(T) 
for  the  trees.  Figure  7  shows  the  coverage  of  the  seed  set,  p(P)  as  new  seeds  are 
generated.  Three  initial  seeds  are  planted  and  two  new  seeds  are  generated  until 
solution  trajectory  is  found.  Therefore  total  5  seeds  are  tried  to  obtain  the  trajectory. 


Fig.  5.  The  modes  of  operation  for  the  aircraft  collision  avoidance  example  (left).  Testing  the 
aircrafts  with  v  =  0.3km/ sec,  u)  =  0.03 rad/ sec,  and  p  =  [4.5,  5.5]km  under  bounded 
disturbances  ||w||  <  0.03km,/ sec  (right).  We  define  the  collision  distance  as  1  km.  Circles 
represent  initial  positions  and  rectangles  are  goal  positions.  A  collision  is  discovered  after 
exploring  about  8,600  nodes  withp  =  5.25 km. 


6.2  Example  2:  Unmanned  blimp  control  law 

In  this  section,  we  consider  the  validation  of  a  feedback  control  algorithm  for  way- 
point  to  waypoint  navigation  of  an  unmanned  outdoor  blimp  under  unpredictable 
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Fig.  6.  Coverage  of  the  trees.  New  trees  are  started  when  the  growth  rate  slows  below  a 
specified  threshold  (g  =  lx  1CP10  used  in  this  example).  Solution  is  discovered  in  one 
of  the  initial  seeds. 


Fig.  7.  The  coverage  improves  (n(P)  decreases)  as  new  trees  are  seeded.  A  collision  is  dis¬ 
covered  at  na=  5. 


but  bounded  disturbances.  The  blimp  has  a  12-dimensional  state  space.  Closed  loop 
control  laws  use  proportional  inertial  feedback  to  keep  the  blimp  at  the  desired  al¬ 
titude  with  the  target  speed  and  to  move  from  one  inertial  waypoint  to  the  next. 
Waypoints  are  generated  in  the  3-dim  space  (x,  y.  z).  Change  of  the  waypoints  can 
be  considered  as  a  system  mode  change.  We  bound  the  input(«(i),  wind  disturbance) 
by  limiting  the  magnitude  of  the  wind  gust  and  the  rate  of  change  of  wind  veloc¬ 
ity.  The  max.  change  rate  of  wind  direction=0.05(l/s)  and  the  max.  change  rate 
of  wind  direction=18°/m  and  the  magnitude  is  bounded  as  ||u||  <  0.03 km/sec. 
For  detailed  description  of  the  feedback  control  law,  sampling  strategy,  metric  de¬ 
sign,  and  the  bounded  wind  disturbance,  refer  to  [15].  In  this  case  the  time  invariant 
parameters  we  will  test  over  is  the  position  of  the  waypoints.  The  waypoints  are 
specified  by  a  high  level  planner  which  does  not  account  for  the  blimp’s  dynamics. 
We  would  like  to  see  if  it  is  possible  for  this  planner  to  specify  a  waypoint  which 
would  cause  the  blimp  to  collide  with  the  obstacle. 
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Figure  8  shows  the  trajectories  of  the  blimp.  The  initial  forward  velocity  is 
0.5  ml  sec.  The  target  forward  velocity  is  1  m/ sec.  The  starting  waypoint  is  [0  0  — 
5]t  and  the  goal  waypoint  is  [150  —  150  —  10]T.  We  assume  the  high  level  planner 
can  generate  intermediate  waypoints  p  £  [80  100]T  x  [—80  —  60]T  x  (—10)  to  avoid 
a  collision.  We  assume  the  navigation  plan  is  achieved  if  the  blimp  can  reach  within 
20m  of  the  goal  waypoint  avoiding  the  obstacle  under  the  wind  disturbance.  A 
counter-example  is  discovered  with  the  intermediate  waypoint  at  [95  —  62.2  —  10]T 
after  exploring  about  9,000  nodes.  The  solution  requires  315  minutes  of  computa¬ 
tion  time  on  1.4GHz  PC.  Coverage  criteria  are  shown  in  Figure  9  and  Figure  10. 
In  this  application,  the  RRFT  analysis  technique  allows  the  designer  to  efficiently 
explore  the  safeness  of  the  blimp  closed  loop  flight  control  laws  for  navigation  plans 
in  the  presence  of  obstacles. 


Fig.  8.  RRFT  of  the  blimp  under  wind  disturbance  ||u||  <  0.03 km/sec  and  uncertain  in¬ 
termediate  waypoints  (left).  Solution  trajectory  is  obtained  after  exploring  about  9000  nodes 
(right) 


Fig.  9.  Coverage  of  the  trees.  New  trees  are  started  when  the  growth  rate  slows  below  a 
specified  threshold  (g  =  lx  10~10  used  in  this  example).  Solution  is  discovered  in  one 
of  the  initial  seeds. 
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Fig.  10.  The  coverage  improves  (p(P)  decreases)  as  new  trees  are  seeded.  A  solution  is  dis¬ 
covered  at  na= 8. 

7  Conclusion 

The  RRT  method  is  a  powerful  technique  to  explore  high-dimensional  configuration 
spaces  and  find  motion  plans  for  systems  with  kinematic  and  dynamic  constraints.  In 
this  paper,  we  presented  two  enhancements  to  this  method  and  a  novel  application. 
First,  we  showed  how  sets  of  time-invariant  parametric  uncertainties  can  be  explored 
with  this  method  to  generate  a  forest  of  trees.  Second,  we  developed  an  on-line 
measure  of  dispersion  that  allows  us  to  adapt  the  growth  of  the  forest  to  the  growth 
rate  of  the  tree.  We  presented  the  application  of  both  methods  to  the  testing  and 
validation  of  hybrid  robot  control  systems,  systems  that  do  not  lend  themselves  to 
proofs  of  convergence  and  stability.  In  both  these  examples,  because  the  controller 
is  fixed,  the  resulting  trees  do  not  expand  to  fill  the  configuration  space.  Instead, 
they  fill  a  ’’tube”  of  configuration  space  that  is  defined  by  allowable  disturbances 
and  external  inputs.  The  first  example  showed  the  ability  to  analyze  multiple-agent 
systems  with  uncertainties,  while  the  second  example  addressed  the  generation  of 
worst  case  disturbances  for  the  analysis  of  full  dynamic  models  of  aerial  vehicles. 
The  adaptation  of  the  growth  of  the  individual  tree  to  coverage  in  configuration 
or  state  space  is  a  direction  of  current  research  and  is  reported  in  a  forthcoming 
publication  [14]. 
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